We use the following code to "unwind" the B axis (i.e. instantly remove multiples of 360 degrees without physically moving the axis):
void unwind_b(void)
{
double p, k;
int n;
p = chan[4].Dest;
n = (int)(p / 360);
k = -n*360.;
while (p+k < -180)
k += 360.;
while (p+k >= 180)
k -= 360.;
WaitNextTimeSlice(); // Avoid interrupts
chan[4].Dest += k;
chan[4].Position += k;
}
Unfortunately, we are getting permanent lost motion in some cases. I suspect we should be adding a call to ResetFilters() in there, to avoid a sudden impulse. But I am not sure when/where to do this. Should it be after the Position increment? Or maybe it is something else?